﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Woodstock.Common.Filters
{
    public class KalmanFilter3
    {
        // Temp values used during Kalman update.
        //private const double dt = 0.0001;
        //private const double dt = 0.019968; //3; //0.0001; //For static data test..
        private const double dt = 0.00809651; //3; //0.0001; //Time for measures average with phidgets test

        private double A_01 = -dt;
        private double AP_00; // = 1;
        private double AP_01;
        private double AP_10;
        private double AP_11;
        private double APAT_00;
        private double APAT_01;
        private double APAT_10;
        private double APAT_11;

        private double B_00 = dt;

        private double inn_00;
        private double K_00;
        private double K_10;
        private double KCPAT_00;
        private double KCPAT_01;
        private double KCPAT_10;
        private double KCPAT_11;
        //0.0998, 0.00002, 0.001, 0.003

        //private const double dt = 0.0001;

        //private double P_00 = 1;
        //private double P_01;
        //private double P_10;
        //private double P_11 = 1;
        private double P_00 = 0.005;
        private double P_01 = 0.005;
        private double P_10 = 0.005;
        private double P_11 = 0.005;


        private double s_00;

        //        private double Sw_00 = 0.0012;
        //        private double Sw_11 = 1.0015;
        private double Sw_00 = 0.005;
        private double Sw_11 = 0.005;

        private double Sz_00 = 17.2;
        //		private double Sz_00 = 6.5;
        private double x_00 = 0.0;
        private double x_10 = 0.0;

        public double Sw00
        {
            get { return Sw_00; }
            set { Sw_00 = value; }
        }

        public double Sw11
        {
            get { return Sw_11; }
            set { Sw_11 = value; }
        }

        public double Sz00
        {
            get { return Sz_00; }
            set { Sz_00 = value; }
        }

        public double Dt
        {
            get { return dt; }
            //            set { dt = value; }
        }

        //private double Sz_00 = 6.5;
        //private double Sw_00 = 0.0007;
        //private double Sw_11 = 0.050;

        public double KalmanUpdate(double gyroRate, double accelAngle)
        // Update the state estimation and compute the Kalman gain.
        // The estimated angle from the output matrix is returned.
        {
            // Update the state estimate by extrapolating current state estimate with input u.
            // x = A * x + B * u
            x_00 += (A_01 * x_10) + (B_00 * gyroRate);

            // Compute the innovation -- error between measured value and state estimate.
            // inn = y - c * x
            inn_00 = accelAngle - x_00;

            // Compute the covariance of the innovation.
            // s = C * P * C' + Sz
            s_00 = P_00 + Sz00;

            // Compute AP matrix for use below.
            // AP = A * P
            AP_00 = P_00 + A_01 * P_10;
            AP_01 = P_01 + A_01 * P_11;
            AP_10 = P_10;
            AP_11 = P_11;

            // Compute the kalman gain matrix.
            // K = A * P * C' * inv(s)
            K_00 = AP_00 / s_00;
            K_10 = AP_10 / s_00;

            // Update the state estimate.
            // x = x + K * inn
            x_00 += K_00 * inn_00;
            x_10 += K_10 * inn_00;

            // Compute the new covariance of the estimation error.
            // P = A * P * A' - K * C * P * A' + Sw
            APAT_00 = AP_00 + (AP_01 * A_01);
            APAT_01 = AP_01;
            APAT_10 = AP_10 + (AP_11 * A_01);
            APAT_11 = AP_11;
            KCPAT_00 = (K_00 * P_00) + (K_00 * P_01) * A_01;
            KCPAT_01 = (K_00 * P_01);
            KCPAT_10 = (K_10 * P_00) + (K_10 * P_01) * A_01;
            KCPAT_11 = (K_10 * P_01);
            P_00 = APAT_00 - KCPAT_00 + Sw00;
            P_01 = APAT_01 - KCPAT_01;
            P_10 = APAT_10 - KCPAT_10;
            P_11 = APAT_11 - KCPAT_11 + Sw11;

            // Return the estimate.
            return x_00;
        }
    }
}
